#include "stdio.h"
#include "string.h"
#include "stdint.h"
#include "signal.h"
#include "unistd.h"
#include "stdlib.h"
#include "pthread.h"

// SOEM库头文件
#include "soem/soem.h"
#include "osal.h"
#include "oshw.h"
#include <inttypes.h>

// -------------------------- 全局配置（需根据实际硬件修改）--------------------------
#define MAX_SLAVES         16      // 最大支持电机数量
#define EC_CYCLETIME       2000000 // 2ms（单位：ns）
#define EC_CYCLETIME_US    (EC_CYCLETIME / 1000) // 转换为us
#define MOTOR_RESOLUTION   131072  // 电机每转脉冲数
#define CIRCLE_CNT         MOTOR_RESOLUTION // 每圈位置值
#define MODE_CSP           0x08    // Cyclic Synchronous Position模式
#define STATUS_PRINT_INTERVAL 2000 // 状态打印间隔（ms）
#define INFO_PRINT_ENABLE  1       // 调试信息开关（1=开启，0=关闭）
#define OP_SWITCH_MAX_RETRY 50     // OP状态切换最大重试次数

// -------------------------- DS402协议关键定义 --------------------------
// 状态字位定义
#define STATUSWORD_MODE_READY  (1 << 8)  // 位8：模式就绪
#define STATUSWORD_FAULT       (1 << 3)  // 位3：故障状态
#define STATUSWORD_SWITCH_ON   (1 << 0)  // 位0：开关使能
#define STATUSWORD_OP_ENABLE   (1 << 1)  // 位1：操作使能
#define STATUSWORD_READY_SWITCH (1 << 2) // 位2：就绪切换
// 控制字定义（分阶段使用）
#define CW_POWER_OFF        0x0006  // 电源关闭（过渡状态）
#define CW_OPERATION_READY  0x000F  // 操作使能+模式确认（CSP激活核心）
#define CW_FAULT_RESET      0x0080  // 故障复位
// PDO映射相关索引
#define PDO_OUTPUT_MAP      0x1600  // 输出PDO映射表索引（RxPdo）
#define PDO_INPUT_MAP       0x1A00  // 输入PDO映射表索引（TxPdo）
#define OBJECT_MODE_DISPLAY 0x6061  // DS402当前模式索引（需映射到PDO）
#define AL_STATUS_CODE      0x1001  // AL状态码索引

// -------------------------- 条件打印宏 --------------------------
#if INFO_PRINT_ENABLE
#define INFO_PRINT(...) printf(__VA_ARGS__)
#else
#define INFO_PRINT(...) do {} while(0)
#endif

// -------------------------- PDO映射结构体（1字节对齐，与从站严格匹配）--------------------------
#pragma pack(push, 1)
// 输出PDO（主站→从站：对应RxPdo）- 13字节
typedef struct {
    uint16_t controlword;           // 0x6040:00 - 控制字 (2字节)
    int8_t   mode_op;               // 0x6060:00 - 控制模式 (1字节)
    int32_t  target_pos;            // 0x607A:00 - 目标位置 (4字节)
    uint16_t touch_probe_func;      // 0x60B8:00 - 触发电平功能 (2字节)
    int32_t  target_vel;            // 0x60FF:00 - 目标速度 (4字节)
        // 添加额外的8字节来匹配实际的21字节大小

} outputs_t;  // 总长度：2+1+4+2+4=13字节

// 输入PDO（从站→主站：对应TxPdo）- 31字节
typedef struct {
    uint16_t error_code;            // 0x603F:00 - 错误码 (2字节)
    uint16_t statusword;            // 0x6041:00 - 状态字 (2字节)
    int8_t   mode_op_dis;           // 0x6061:00 - 当前模式 (1字节)
    int32_t  actual_pos;            // 0x6064:00 - 实际位置 (4字节)
    int32_t  actual_vel;            // 0x606C:00 - 实际速度 (4字节)
    uint16_t touch_probe_status;    // 0x60B9:00 - 触发电平状态 (2字节)
    // 由于实际只有20字节，移除不需要的字段，添加5字节填充

    int32_t  touch_probe_pos1;      // 0x60BA:00 - 触发位置1 (4字节)
    int32_t  touch_probe_pos2;      // 0x60BC:00 - 触发位置2 (4字节)
    int32_t  following_error;       // 0x60F4:00 - 跟随误差 (4字节)
    uint32_t digital_inputs;        // 0x60FD:00 - 数字输入 (4字节)
} inputs_t;  // 总长度：2+2+1+4+4+2+4+4+4+4=31字节
#pragma pack(pop)




// // -------------------------- PDO映射结构体（1字节对齐，与从站严格匹配）--------------------------
// #pragma pack(push, 1)
// // 输出PDO（主站→从站：对应RxPdo）- 13字节
// typedef struct {
//     uint16_t controlword;           // 0x6040:00 - 控制字 (2字节)
//     // int8_t   mode_op;               // 0x6060:00 - 控制模式 (1字节)
//     int32_t  target_pos;            // 0x607A:00 - 目标位置 (4字节)
//     uint16_t touch_probe_func;      // 0x60B8:00 - 触发电平功能 (2字节)
//     // int32_t  target_vel;            // 0x60FF:00 - 目标速度 (4字节)
//         // 添加额外的8字节来匹配实际的21字节大小
//     uint8_t  padding[13];            // 填充字节 (8字节)
// } outputs_t;  // 总长度：2+1+4+2+4=13字节

// // 输入PDO（从站→主站：对应TxPdo）- 31字节
// typedef struct {
    
//     uint16_t statusword;            // 0x6041:00 - 状态字 (2字节)
//     // int8_t   mode_op_dis;           // 0x6061:00 - 当前模式 (1字节)
//     int32_t  actual_pos;            // 0x6064:00 - 实际位置 (4字节)
//     // int32_t  actual_vel;            // 0x606C:00 - 实际速度 (4字节)
//     uint16_t touch_probe_status;    // 0x60B9:00 - 触发电平状态 (2字节)
//     int32_t  touch_probe_pos1;      // 0x60BA:00 - 触发位置1 (4字节)
//     int32_t  touch_probe_pos2;      // 0x60BC:00 - 触发位置2 (4字节)
//     uint16_t error_code;            // 0x603F:00 - 错误码 (2字节)
//     uint32_t digital_inputs;        // 0x60FD:00 - 数字输入 (4字节)
//     // 由于实际只有20字节，移除不需要的字段，添加5字节填充
//     // uint8_t  padding[5];            // 填充字节 (5字节)
   
   
//     // int32_t  following_error;       // 0x60F4:00 - 跟随误差 (4字节)
 
// } inputs_t;  // 总长度：2+2+1+4+4+2+4+4+4+4=31字节
// #pragma pack(pop)

// -------------------------- 电机配置结构体 --------------------------
typedef struct {
    int slave_index;         // 从站索引（1-based）
    uint8* outputs;          // 输出PDO地址指针
    uint8* inputs;           // 输入PDO地址指针
    int Obytes;              // 输出PDO字节数
    int Ibytes;              // 输入PDO字节数
    boolean is_operational;  // 从站是否OP状态
    // 控制参数
    int32_t target_pos;      // 目标位置
    int32_t actual_pos;      // 实际位置（从RPDO读取）
    int32_t base_pos_for_increment; // 第500周期基准位置
    int cycle_cnt;           // 电机独立周期计数
    // 状态跟踪
    int fault_cleared;       // 故障标记（0=未清除，1=已清除）
    int fault_reset_cnt;     // 故障复位计数器
    char name[32];           // 电机名称
    int last_error_code;     // 上一次错误码
    // 从站配置信息
    uint32_t vendor_id;      // 厂商ID
    uint32_t product_code;   // 产品代码
    uint32_t revision_num;   // 版本号
    uint32_t serial_num;     // 序列号
} motor_config_t;

// -------------------------- 全局变量 --------------------------
ecx_contextt ecx_context;       // SOEM上下文
uint8_t      IOmap[4096];        // PDO映射缓存
pthread_t    thread_monitor;     // 从站状态监控线程
pthread_t    thread_status_printer; // 状态打印线程
static motor_config_t motor_configs[MAX_SLAVES]; // 电机配置数组
static int motor_count = 0;      // 实际配置电机数量
// 控制状态变量
volatile int  wkc;               // 工作计数器
int           expectedWKC;       // 预期工作计数器
boolean       inOP = FALSE;      // 主站是否OP状态
boolean       needlf = FALSE;    // 日志换行标记
uint8_t       current_group = 0; // 同步组（默认0）
volatile boolean exit_flag = FALSE; // 退出标志
volatile int motor_csp_ready[MAX_SLAVES] = {0}; // 电机CSP就绪标志
static int op_switch_fail_cnt[MAX_SLAVES] = {0}; // OP切换失败计数器

// -------------------------- 函数声明 --------------------------
void *slave_monitor(void *ptr);
void *status_printer_thread(void *ptr);
void csp_control_main(char *ifname);
void print_usage();
void signal_handler(int sig);
// 电机控制核心函数
void init_motor_configs(void);
void setup_motor_control_parameters(int motor_idx);
void control_single_motor(int motor_idx);
void print_motor_status(int motor_idx);
// 诊断与辅助函数
void detailed_pdo_analysis(void);
void print_all_slave_status(void);
void print_ecat_rawdata_pdo(int global_cycle);
boolean validate_motor_data_access(int motor_idx);
const char* ec_state_to_string(uint16_t state);
void print_pdo_sizes(void);
void read_slave_configurations(void);
void read_slave_basic_info(int slave_idx, motor_config_t *motor);
void read_slave_pdo_mappings(int slave_idx);
void read_slave_sm_config(int slave_idx);
int clear_slave_al_error(int slave_idx);
void configure_slave_pdo_mapping(int slave_idx); // 新增：配置PDO映射

// -------------------------- 从站状态转字符串 --------------------------
const char* ec_state_to_string(uint16_t state) {
    switch(state) {
        case EC_STATE_INIT: return "INIT";
        case EC_STATE_PRE_OP: return "PRE_OP";
        case EC_STATE_SAFE_OP: return "SAFE_OP";
        case EC_STATE_OPERATIONAL: return "OPERATIONAL";
        case EC_STATE_SAFE_OP + EC_STATE_ERROR: return "SAFE_OP+ERROR";
        case EC_STATE_NONE: return "NONE (Lost)";
        default: return "UNKNOWN";
    }
}

// -------------------------- 主函数 --------------------------
int main(int argc, char **argv) {
    signal(SIGINT, signal_handler);
    INFO_PRINT("===================== EtherCAT双电机CSP控制程序 =====================\n");

    if (argc != 2) {
        print_usage();
        ec_adaptert *adapter = ec_find_adapters();
        ec_adaptert *adapter_head = adapter;
        if (adapter != NULL) {
            INFO_PRINT("\n可用以太网适配器:\n");
            while (adapter != NULL) {
                INFO_PRINT("  - %s  (%s)\n", adapter->name, adapter->desc);
                adapter = adapter->next;
            }
        } else {
            INFO_PRINT("\n未发现可用以太网适配器!\n");
        }
        ec_free_adapters(adapter_head);
        return -1;
    }

    if (pthread_create(&thread_monitor, NULL, slave_monitor, NULL) != 0) {
        INFO_PRINT("ERROR: 创建从站监控线程失败!\n");
        return -1;
    }
    if (pthread_create(&thread_status_printer, NULL, status_printer_thread, NULL) != 0) {
        INFO_PRINT("ERROR: 创建状态打印线程失败!\n");
        pthread_join(thread_monitor, NULL);
        return -1;
    }

    csp_control_main(argv[1]);

    inOP = FALSE;
    pthread_join(thread_monitor, NULL);
    pthread_join(thread_status_printer, NULL);
    INFO_PRINT("\n===================== 程序正常退出 =====================\n");
    return 0;
}

// -------------------------- 配置从站PDO映射 --------------------------
void configure_slave_pdo_mapping(int slave_idx) {
    int ret;
    uint32_t pdo_entry;
    
    INFO_PRINT("配置从站 %d 的PDO映射...\n", slave_idx);
    
    // 配置输出PDO (RxPDO - 主站→从站)
    // 清除现有的PDO映射
    uint8_t zero = 0;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1C12, 0x00, FALSE, sizeof(uint8_t), &zero, EC_TIMEOUTRXM);
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1C13, 0x00, FALSE, sizeof(uint8_t), &zero, EC_TIMEOUTRXM);
    
    // 配置输出PDO映射 (0x1600)
    uint8_t pdo_entries_count = 5; // 5个对象
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1600, 0x00, FALSE, sizeof(pdo_entries_count), &pdo_entries_count, EC_TIMEOUTRXM);
    
    // 控制字 0x6040:00
    pdo_entry = (0x6040 << 16) | (0x00 << 8) | 16;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1600, 0x01, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 控制模式 0x6060:00  
    pdo_entry = (0x6060 << 16) | (0x00 << 8) | 8;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1600, 0x02, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 目标位置 0x607A:00
    pdo_entry = (0x607A << 16) | (0x00 << 8) | 32;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1600, 0x03, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 触发电平功能 0x60B8:00
    pdo_entry = (0x60B8 << 16) | (0x00 << 8) | 16;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1600, 0x04, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 目标速度 0x60FF:00
    pdo_entry = (0x60FF << 16) | (0x00 << 8) | 32;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1600, 0x05, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 配置输入PDO映射 (0x1A00)
    pdo_entries_count = 10; // 10个对象
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x00, FALSE, sizeof(pdo_entries_count), &pdo_entries_count, EC_TIMEOUTRXM);
    
    // 错误码 0x603F:00
    pdo_entry = (0x603F << 16) | (0x00 << 8) | 16;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x01, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 状态字 0x6041:00
    pdo_entry = (0x6041 << 16) | (0x00 << 8) | 16;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x02, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 当前模式 0x6061:00
    pdo_entry = (0x6061 << 16) | (0x00 << 8) | 8;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x03, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 实际位置 0x6064:00
    pdo_entry = (0x6064 << 16) | (0x00 << 8) | 32;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x04, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 实际速度 0x606C:00
    pdo_entry = (0x606C << 16) | (0x00 << 8) | 32;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x05, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 触发电平状态 0x60B9:00
    pdo_entry = (0x60B9 << 16) | (0x00 << 8) | 16;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x06, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 触发位置1 0x60BA:00
    pdo_entry = (0x60BA << 16) | (0x00 << 8) | 32;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x07, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 触发位置2 0x60BC:00
    pdo_entry = (0x60BC << 16) | (0x00 << 8) | 32;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x08, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 跟随误差 0x60F4:00
    pdo_entry = (0x60F4 << 16) | (0x00 << 8) | 32;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x09, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    // 数字输入 0x60FD:00
    pdo_entry = (0x60FD << 16) | (0x00 << 8) | 32;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x1A00, 0x0A, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
    
    INFO_PRINT("从站 %d PDO映射配置完成\n", slave_idx);
}

// -------------------------- 从站状态监控线程（优化版）--------------------------
void *slave_monitor(void *ptr) {
    (void)ptr;
    int slave_idx;

    while (!exit_flag) {
        if (inOP && ((wkc < expectedWKC) || ecx_context.grouplist[current_group].docheckstate)) {
            if (needlf) {
                needlf = FALSE;
                INFO_PRINT("\n");
            }

            ecx_context.grouplist[current_group].docheckstate = FALSE;
            ecx_readstate(&ecx_context);

            for (slave_idx = 1; slave_idx <= ecx_context.slavecount; slave_idx++) {
                ec_slavet *slave = &ecx_context.slavelist[slave_idx];
                if (slave->group != current_group) continue;

                if (slave->state != EC_STATE_OPERATIONAL) {
                    ecx_context.grouplist[current_group].docheckstate = TRUE;
                    for (int i = 0; i < motor_count; i++) {
                        if (motor_configs[i].slave_index == slave_idx) {
                            motor_configs[i].is_operational = FALSE;
                            break;
                        }
                    }

                    // 增强：打印AL状态码用于诊断
                    INFO_PRINT("DEBUG: Slave %d 状态=%s, AL状态码=0x%04X\n",
                           slave_idx, ec_state_to_string(slave->state), slave->ALstatuscode);

                    if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) {
                        INFO_PRINT("ERROR: Slave %d(Motor %d) 状态: [SAFE_OP+ERROR]，监控603F错误码...\n", 
                                 slave_idx, slave_idx-1);
                        op_switch_fail_cnt[slave_idx] = 0;
                    } 
                    // 优化：SAFE_OP状态处理逻辑
                    else if (slave->state == EC_STATE_SAFE_OP) {
                        op_switch_fail_cnt[slave_idx]++;
                        
                        // 超过最大重试次数，提示可能原因
                        if (op_switch_fail_cnt[slave_idx] >= OP_SWITCH_MAX_RETRY) {
                            INFO_PRINT("ERROR: Slave %d 切换到OP失败超过%d次！可能原因：\n", 
                                   slave_idx, OP_SWITCH_MAX_RETRY);
                            INFO_PRINT("  1. 硬件故障（电机/驱动器接线错误）\n");
                            INFO_PRINT("  2. PDO映射不匹配（检查输入输出PDO大小）\n");
                            INFO_PRINT("  3. AL状态码0x%04X对应的设备特定错误（参考手册）\n", 
                                   slave->ALstatuscode);
                            op_switch_fail_cnt[slave_idx] = 0;
                        }

                        // 先清除AL错误再尝试切换
                        if (slave->ALstatuscode != 0) {
                            INFO_PRINT("WARNING: Slave %d 存在AL错误(0x%04X)，尝试清除...\n",
                                   slave_idx, slave->ALstatuscode);
                            if (clear_slave_al_error(slave_idx) == 1) {
                                INFO_PRINT("INFO: Slave %d AL错误清除成功，等待状态更新...\n", slave_idx);
                                osal_usleep(100000); // 等待100ms
                                ecx_readstate(&ecx_context);
                            }
                        }

                        // AL状态正常时尝试切换到OP
                        if (slave->ALstatuscode == 0) {
                            INFO_PRINT("INFO: Slave %d(Motor %d) 尝试切换到OP...\n", 
                                   slave_idx, slave_idx-1);
                            slave->state = EC_STATE_OPERATIONAL;
                            ecx_writestate(&ecx_context, slave_idx);
                            osal_usleep(50000); // 等待50ms确认
                            ecx_readstate(&ecx_context);
                            
                            if (slave->state == EC_STATE_OPERATIONAL) {
                                INFO_PRINT("SUCCESS: Slave %d 成功进入OP状态！\n", slave_idx);
                                op_switch_fail_cnt[slave_idx] = 0;
                            }
                        }
                    } 
                    else if (slave->state > EC_STATE_NONE && ecx_reconfig_slave(&ecx_context, slave_idx, EC_TIMEOUTRET)) {
                        slave->islost = FALSE;
                        INFO_PRINT("INFO: Slave %d(Motor %d) 重新配置成功!\n", slave_idx, slave_idx-1);
                        op_switch_fail_cnt[slave_idx] = 0;
                    } 
                    else if (!slave->islost) {
                        ecx_statecheck(&ecx_context, slave_idx, EC_STATE_OPERATIONAL, EC_TIMEOUTRET * 5);
                        if (slave->state == EC_STATE_NONE) {
                            slave->islost = TRUE;
                            INFO_PRINT("ERROR: Slave %d(Motor %d) 丢失!\n", slave_idx, slave_idx-1);
                            op_switch_fail_cnt[slave_idx] = 0;
                        }
                    }
                } 
                // 从站恢复检测
                else if (slave->islost) {
                    slave->islost = FALSE;
                    for (int i = 0; i < motor_count; i++) {
                        if (motor_configs[i].slave_index == slave_idx) {
                            motor_configs[i].is_operational = TRUE;
                            break;
                        }
                    }
                    INFO_PRINT("INFO: Slave %d(Motor %d) 恢复正常!\n", slave_idx, slave_idx-1);
                    op_switch_fail_cnt[slave_idx] = 0;
                }
            }

            if (!ecx_context.grouplist[current_group].docheckstate) {
                INFO_PRINT("INFO: 所有从站恢复 [OPERATIONAL] 状态!\n");
                for (int i = 0; i < motor_count; i++) {
                    motor_configs[i].is_operational = TRUE;
                }
            }
        }
        osal_usleep(10000); // 10ms检查一次
    }
    return NULL;
}

// -------------------------- 状态打印线程 --------------------------
void *status_printer_thread(void *ptr) {
    (void)ptr;
    while (!exit_flag) {
        osal_usleep(STATUS_PRINT_INTERVAL * 1000);
        if (ecx_context.slavecount > 0) {
            print_all_slave_status();
        }
    }
    return NULL;
}

// -------------------------- 全量状态打印 --------------------------
void print_all_slave_status(void) {
    ecx_readstate(&ecx_context);
    ecx_statecheck(&ecx_context, 0, EC_STATE_OPERATIONAL, 0);
    
    INFO_PRINT("\n===================== 从站状态快照 =====================\n");
    struct timeval tv;
    gettimeofday(&tv, NULL);
    uint64_t timestamp_ms = (uint64_t)tv.tv_sec * 1000 + tv.tv_usec / 1000;
    INFO_PRINT("时间戳: %lu ms\n", (unsigned long)timestamp_ms);
    
    int all_op = 1;
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        if (ecx_context.slavelist[i].state != EC_STATE_OPERATIONAL) {
            all_op = 0;
            break;
        }
    }
    INFO_PRINT("总从站数: %d | 全部OP: %s\n", 
           ecx_context.slavecount, all_op ? "YES" : "NO");
    INFO_PRINT("-----------------------------------------------------------------\n");
    INFO_PRINT("%-6s %-20s %-15s %-8s %-10s %s\n", 
           "Slave", "名称", "状态", "WKC", "是否丢失", "AL错误码");
    INFO_PRINT("-----------------------------------------------------------------\n");
    
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        ec_slavet *slave = &ecx_context.slavelist[i];
        INFO_PRINT("%-6d %-20s %-15s %-8d %-10s 0x%04X\n",
               i,
               slave->name,
               ec_state_to_string(slave->state),
               wkc,
               slave->islost ? "YES" : "NO",
               slave->ALstatuscode);
    }
    
    INFO_PRINT("-----------------------------------------------------------------\n");
    INFO_PRINT("电机DS402状态汇总:\n");
    for (int i = 0; i < motor_count; i++) {
        inputs_t *in_ptr = (inputs_t *)motor_configs[i].inputs;
        int mode_ready = 0, has_fault = 0;
        uint16_t error_code = 0;
        if (in_ptr) {
            mode_ready = (in_ptr->statusword & STATUSWORD_MODE_READY) ? 1 : 0;
            has_fault = (in_ptr->statusword & STATUSWORD_FAULT) ? 1 : 0;
            error_code = in_ptr->error_code;
            
            if (error_code != motor_configs[i].last_error_code) {
                INFO_PRINT("  ALERT: Motor %d 603F错误码变化 - 旧:0x%04X → 新:0x%04X\n",
                       i, motor_configs[i].last_error_code, error_code);
                motor_configs[i].last_error_code = error_code;
            }
        }
        
        INFO_PRINT("  Motor %d: OP=%-3s | 故障=%-1d | 模式就绪=%-1d | 当前模式=0x%02X | 实际位置=%8d | 状态字=0x%04X | 错误码=0x%04X\n",
               i,
               motor_configs[i].is_operational ? "YES" : "NO",
               has_fault,
               mode_ready,
               in_ptr ? in_ptr->mode_op_dis : 0xFF,
               motor_configs[i].actual_pos,
               in_ptr ? in_ptr->statusword : 0xFFFF,
               error_code);
    }
    INFO_PRINT("=================================================================\n\n");
}

// -------------------------- 电机数据访问验证 --------------------------
boolean validate_motor_data_access(int motor_idx) {
    if (motor_idx < 0 || motor_idx >= motor_count) {
        return FALSE;
    }
    if (motor_configs[motor_idx].outputs == NULL || motor_configs[motor_idx].inputs == NULL) {
        INFO_PRINT("WARNING: Motor %d PDO指针为空!\n", motor_idx);
        return FALSE;
    }
    // 更新为13/31字节的检查
    if (motor_configs[motor_idx].Obytes < sizeof(outputs_t) || 
        motor_configs[motor_idx].Ibytes < sizeof(inputs_t)) {
        INFO_PRINT("WARNING: Motor %d PDO大小不匹配（需O:%lu/I:%lu，实际O:%d/I:%d）\n", 
               motor_idx, sizeof(outputs_t), sizeof(inputs_t),
               motor_configs[motor_idx].Obytes, motor_configs[motor_idx].Ibytes);
        return FALSE;
    }
    return TRUE;
}

// -------------------------- 初始化电机配置 --------------------------
void init_motor_configs(void) {
    motor_count = 0;
    for (int i = 1; i <= ecx_context.slavecount && motor_count < MAX_SLAVES; i++) {
        motor_config_t *motor = &motor_configs[motor_count];
        motor->slave_index = i;
        motor->Obytes = ecx_context.slavelist[i].Obytes;
        motor->Ibytes = ecx_context.slavelist[i].Ibytes;
        motor->outputs = ecx_context.slavelist[i].outputs;
        motor->inputs = ecx_context.slavelist[i].inputs;
        motor->is_operational = FALSE;
        motor->fault_cleared = 0;
        motor->fault_reset_cnt = 0;
        motor->last_error_code = 0;
        motor->target_pos = 0;
        motor->actual_pos = 0;
        motor->base_pos_for_increment = 0;
        motor->cycle_cnt = 0;
        motor_csp_ready[motor_count] = 0;
        read_slave_basic_info(i, motor);
        setup_motor_control_parameters(motor_count);
        snprintf(motor->name, 32, "Motor-%d", motor_count);
        INFO_PRINT("  Motor %d: Slave %d | 输出地址=0x%lx | 输入地址=0x%lx | O:%d字节 | I:%d字节\n", 
               motor_count, i, 
               (unsigned long)motor->outputs, (unsigned long)motor->inputs,
               motor->Obytes, motor->Ibytes);
        motor_count++;
    }
    INFO_PRINT("INFO: 共配置 %d 台电机\n", motor_count);
}

// -------------------------- 电机控制参数配置 --------------------------
void setup_motor_control_parameters(int motor_idx) {
    motor_config_t *motor = &motor_configs[motor_idx];
    int slave_idx = motor->slave_index;
    int ret = 0;

    int8_t csp_mode = MODE_CSP;
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x6060, 0x00, FALSE, 
                      sizeof(csp_mode), &csp_mode, EC_TIMEOUTRXM);
    if (ret == 1) {
        INFO_PRINT("SUCCESS: Motor %d (Slave %d) SDO配置0x6060:00=CSP(0x08)成功\n", 
               motor_idx, slave_idx);
    } else {
        INFO_PRINT("ALERT: Motor %d (Slave %d) SDO配置CSP模式失败！\n", motor_idx, slave_idx);
    }

    uint16_t init_cw = 0x0006; // Shutdown
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x6040, 0x00, FALSE, 
                      sizeof(init_cw), &init_cw, EC_TIMEOUTRXM);
    if (ret == 1) {
        INFO_PRINT("INFO: Motor %d (Slave %d) SDO初始化控制字=0x%04X成功\n", 
               motor_idx, slave_idx, init_cw);
    }
}

// -------------------------- 单电机CSP控制（优化故障复位）--------------------------
void control_single_motor(int motor_idx) {
    if (!validate_motor_data_access(motor_idx)) {
        return;
    }

    motor_config_t *motor = &motor_configs[motor_idx];
    outputs_t *out_ptr = (outputs_t *)motor->outputs;
    inputs_t  *in_ptr = (inputs_t *)motor->inputs;

    out_ptr->mode_op = MODE_CSP;
    out_ptr->touch_probe_func = 0x0000;

    motor->cycle_cnt++;
    int current_cycle = motor->cycle_cnt;

    uint16_t status = in_ptr ? in_ptr->statusword : 0x0000;
    uint16_t error_code = in_ptr ? in_ptr->error_code : 0x0000;
    int has_fault = (status & STATUSWORD_FAULT) ? 1 : 0;

    // 优化：增强故障复位逻辑，连续3次发送复位指令
    if (has_fault && in_ptr) {
        if (error_code != motor->last_error_code) {
            // INFO_PRINT("ALERT: Motor %d (周期:%d) - 故障状态(0x%04X) | 603F错误码:0x%04X\n",
            //        motor_idx, current_cycle, status, error_code);
            motor->last_error_code = error_code;
        }

        // 连续3次发送故障复位
        if (motor->fault_reset_cnt < 3) {
            out_ptr->controlword = CW_FAULT_RESET;
            motor->fault_reset_cnt++;
            // INFO_PRINT("INFO: Motor %d 发送故障复位（第%d次）\n", motor_idx, motor->fault_reset_cnt);
        } else {
            // 复位后回到Shutdown状态重新检测
            out_ptr->controlword = 0x0006;
            motor->fault_reset_cnt = 0;
            motor->fault_cleared = (error_code == 0) ? 1 : 0;
            // INFO_PRINT("INFO: Motor %d 完成3次复位，故障状态:%d\n", motor_idx, has_fault);
        }
        return;
    }

    // 目标位置计算
    if (in_ptr != NULL) {
        motor->actual_pos = in_ptr->actual_pos;
        if (current_cycle <= 500) {
            motor->target_pos = motor->actual_pos;
            if (current_cycle == 500) {
                motor->base_pos_for_increment = motor->actual_pos;
                INFO_PRINT("INFO: Motor %d - 第500周期，记录基准位置: %d\n",
                       motor_idx, motor->base_pos_for_increment);
            }
        } else {
            motor->target_pos = motor->base_pos_for_increment + (current_cycle - 500) * 10;
        }
    } else {
        if (current_cycle == 500) {
            motor->base_pos_for_increment = 0;
            INFO_PRINT("WARNING: Motor %d - 无法读取实际位置，基准位置设为0\n", motor_idx);
        } else if (current_cycle > 500) {
            motor->target_pos = motor->base_pos_for_increment + (current_cycle - 500) * 10;
        }
    }
    out_ptr->target_pos = motor->target_pos;
    out_ptr->target_vel = 0;

    // DS402控制字使能序列
    if (!motor->fault_cleared) {
        out_ptr->controlword = 0x0006;
        if (!has_fault) {
            motor->fault_cleared = 1;
            INFO_PRINT("INFO: Motor %d - 无故障，标记故障已清除 (周期:%d)\n",
                   motor_idx, current_cycle);
        }
    } else if (!motor_csp_ready[motor_idx]) {
        if (current_cycle < 100) {
            out_ptr->controlword = 0x0006;
            if (current_cycle == 1) {
                INFO_PRINT("INFO: Motor %d - 步骤1/4：Shutdown (周期:%d)\n", motor_idx, current_cycle);
            }
        } else if (current_cycle < 200) {
            out_ptr->controlword = 0x0007;
            if (current_cycle == 100) {
                INFO_PRINT("INFO: Motor %d - 步骤2/4：Switch on (控制字:0x0007, 周期:%d)\n",
                       motor_idx, current_cycle);
            }
        } else if (current_cycle < 300) {
            out_ptr->controlword = 0x000F;
            if (current_cycle == 200) {
                INFO_PRINT("INFO: Motor %d - 步骤3/4：Enable operation (控制字:0x000F, 周期:%d)\n",
                       motor_idx, current_cycle);
            }
        } else {
            out_ptr->controlword = 0x000F;
            
            if (in_ptr) {
                uint16_t status = in_ptr->statusword;
                int mode_ready = (status & STATUSWORD_MODE_READY) ? 1 : 0;
                int switch_on = (status & STATUSWORD_SWITCH_ON) ? 1 : 0;
                int op_enabled = (status & STATUSWORD_OP_ENABLE) ? 1 : 0;
                int current_mode = in_ptr->mode_op_dis;
                
                if (mode_ready && op_enabled && current_mode == MODE_CSP) {
                    motor_csp_ready[motor_idx] = 1;
                    INFO_PRINT("SUCCESS: Motor %d - CSP模式激活成功! 状态字=0x%04X 模式=0x%02X (周期:%d)\n",
                           motor_idx, status, current_mode, current_cycle);
                }
                
                if (current_cycle % 200 == 0) {
                    INFO_PRINT("Motor %d 等待CSP: 状态字=0x%04X(RS=%d,SO=%d,OE=%d) 模式=0x%02X 周期=%d\n",
                           motor_idx, status, 
                           (status & STATUSWORD_READY_SWITCH) ? 1 : 0,
                           switch_on, op_enabled,
                           current_mode, current_cycle);
                }
            }
            
            if (current_cycle > 5000) {
                motor->cycle_cnt = 0;
                INFO_PRINT("WARNING: Motor %d - CSP激活超时，重置周期计数 (周期:%d)\n",
                       motor_idx, current_cycle);
            }
        }
    } else {
        out_ptr->controlword = 0x000F;
    }
}

// -------------------------- 电机状态简印 --------------------------
void print_motor_status(int motor_idx) {
    if (!validate_motor_data_access(motor_idx)) {
        INFO_PRINT("M%d:无效 ", motor_idx);
        return;
    }
    
    inputs_t *in_ptr = (inputs_t *)motor_configs[motor_idx].inputs;
    uint16_t status = in_ptr->statusword;
    
    int ready_switch = (status & STATUSWORD_READY_SWITCH) ? 1 : 0;
    int switch_on = (status & STATUSWORD_SWITCH_ON) ? 1 : 0;
    int op_enable = (status & STATUSWORD_OP_ENABLE) ? 1 : 0;
    int fault = (status & STATUSWORD_FAULT) ? 1 : 0;
    int mode_ready = (status & STATUSWORD_MODE_READY) ? 1 : 0;
    
    INFO_PRINT("M%d:Pos=%6d St=0x%04X(RS%dS%dO%dF%dM%d) Mode=0x%02X | ", 
           motor_idx,
           motor_configs[motor_idx].actual_pos,
           status,
           ready_switch, switch_on, op_enable, fault, mode_ready,
           in_ptr->mode_op_dis);
}

// -------------------------- PDO详细分析 --------------------------
void detailed_pdo_analysis(void) {
    INFO_PRINT("\n=== PDO详细分析 ===\n");
    INFO_PRINT("主站PDO总大小:\n");
    INFO_PRINT("  输出: %d bytes | 输入: %d bytes\n", 
           ecx_context.slavelist[0].Obytes, ecx_context.slavelist[0].Ibytes);
    INFO_PRINT("各从站PDO大小:\n");
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        ec_slavet *slave = &ecx_context.slavelist[i];
        INFO_PRINT("  Slave %d (%s):\n", i, slave->name);
        INFO_PRINT("    输出: %d位(%d字节) | 输入: %d位(%d字节)\n", 
               slave->Obits, slave->Obytes, slave->Ibits, slave->Ibytes);
        for (int sm = 2; sm <= 3; sm++) {
            if (slave->SM[sm].StartAddr > 0) {
                INFO_PRINT("    SM%d: 起始地址=0x%04X | 长度=%d字节 | 类型=%d\n", 
                       sm, slave->SM[sm].StartAddr, slave->SM[sm].SMlength, slave->SMtype[sm]);
            }
        }
    }
    int total_out = 0, total_in = 0;
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        total_out += ecx_context.slavelist[i].Obytes;
        total_in += ecx_context.slavelist[i].Ibytes;
    }
    INFO_PRINT("大小校验:\n");
    INFO_PRINT("  输出总和: %d bytes (主站总大小: %d bytes | 差异: %d bytes)\n",
           total_out, ecx_context.slavelist[0].Obytes, ecx_context.slavelist[0].Obytes - total_out);
    INFO_PRINT("  输入总和: %d bytes (主站总大小: %d bytes | 差异: %d bytes)\n",
           total_in, ecx_context.slavelist[0].Ibytes, ecx_context.slavelist[0].Ibytes - total_in);
    INFO_PRINT("\nPDO地址偏移:\n");
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        ec_slavet *slave = &ecx_context.slavelist[i];
        INFO_PRINT("  Slave %d:\n", i);
        INFO_PRINT("    输出偏移: %ld | 输入偏移: %ld\n", 
               slave->outputs - IOmap, slave->inputs - IOmap);
        if (i > 1) {
            ec_slavet *prev = &ecx_context.slavelist[i-1];
            int out_gap = slave->outputs - (prev->outputs + prev->Obytes);
            int in_gap = slave->inputs - (prev->inputs + prev->Ibytes);
            if (out_gap < 0) INFO_PRINT("    WARNING: 输出PDO与Slave %d重叠（间隙:%d字节）\n", i-1, out_gap);
            if (in_gap < 0) INFO_PRINT("    WARNING: 输入PDO与Slave %d重叠（间隙:%d字节）\n", i-1, in_gap);
        }
    }
}

// -------------------------- 打印从站实际PDO大小 --------------------------
void print_pdo_sizes() {
    INFO_PRINT("\n===== 从站实际PDO大小（SOEM读取值） =====");
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        ec_slavet *slave = &ecx_context.slavelist[i];
        INFO_PRINT("\n从站 %d（名称：%s）:", i, slave->name);
        INFO_PRINT("\n  输出PDO（RxPDO）大小：%d 字节（Obytes）", slave->Obytes);
        INFO_PRINT("\n  输入PDO（TxPDO）大小：%d 字节（Ibytes）", slave->Ibytes);
        INFO_PRINT("\n  输出PDO位数：%d 位（Obits）", slave->Obits);
        INFO_PRINT("\n  输入PDO位数：%d 位（Ibits）", slave->Ibits);
    }
    INFO_PRINT("\n=========================================\n");
}

// -------------------------- 读取从站基本信息 --------------------------
void read_slave_basic_info(int slave_idx, motor_config_t *motor) {
    uint32_t data;
    int ret;
    
    ret = ecx_SDOread(&ecx_context, slave_idx, 0x0001, 0x00, FALSE, sizeof(data), &data, EC_TIMEOUTRXM);
    if (ret == 1) {
        motor->vendor_id = data;
        INFO_PRINT("INFO: Slave %d 厂商ID: 0x%08X\n", slave_idx, data);
    } else {
        motor->vendor_id = 0;
        INFO_PRINT("WARNING: Slave %d 读取厂商ID失败\n", slave_idx);
    }
    
    ret = ecx_SDOread(&ecx_context, slave_idx, 0x0002, 0x00, FALSE, sizeof(data), &data, EC_TIMEOUTRXM);
    if (ret == 1) {
        motor->product_code = data;
        INFO_PRINT("INFO: Slave %d 产品代码: 0x%08X\n", slave_idx, data);
    } else {
        motor->product_code = 0;
        INFO_PRINT("WARNING: Slave %d 读取产品代码失败\n", slave_idx);
    }
    
    ret = ecx_SDOread(&ecx_context, slave_idx, 0x0003, 0x00, FALSE, sizeof(data), &data, EC_TIMEOUTRXM);
    if (ret == 1) {
        motor->revision_num = data;
        INFO_PRINT("INFO: Slave %d 版本号: 0x%08X\n", slave_idx, data);
    } else {
        motor->revision_num = 0;
        INFO_PRINT("WARNING: Slave %d 读取版本号失败\n", slave_idx);
    }
    
    ret = ecx_SDOread(&ecx_context, slave_idx, 0x0006, 0x00, FALSE, sizeof(data), &data, EC_TIMEOUTRXM);
    if (ret == 1) {
        motor->serial_num = data;
        INFO_PRINT("INFO: Slave %d 序列号: 0x%08X\n", slave_idx, data);
    } else {
        motor->serial_num = 0;
        INFO_PRINT("WARNING: Slave %d 读取序列号失败\n", slave_idx);
    }
}

// -------------------------- 读取从站PDO映射配置 --------------------------
void read_slave_pdo_mappings(int slave_idx) {
    uint32_t pdo_entry;
    int ret, idx;
    
    INFO_PRINT("\n===== 从站 %d PDO映射配置 =====", slave_idx);
    
    INFO_PRINT("\n输出PDO (RxPDO) 映射:");
    for (idx = 0x1600; idx <= 0x160F; idx++) {
        ret = ecx_SDOread(&ecx_context, slave_idx, idx, 0x00, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
        if (ret == 1 && pdo_entry != 0) {
            uint16_t obj_index = (pdo_entry >> 16) & 0xFFFF;
            uint8_t  sub_index = (pdo_entry >> 8) & 0xFF;
            uint8_t  length = pdo_entry & 0xFF;
            INFO_PRINT("\n  0x%04X:00 = 0x%08X -> 对象0x%04X:%02X, 长度%d位", 
                   idx, pdo_entry, obj_index, sub_index, length);
        } else if (pdo_entry == 0) {
            break;
        }
    }
    
    INFO_PRINT("\n输入PDO (TxPDO) 映射:");
    for (idx = 0x1A00; idx <= 0x1A0F; idx++) {
        ret = ecx_SDOread(&ecx_context, slave_idx, idx, 0x00, FALSE, sizeof(pdo_entry), &pdo_entry, EC_TIMEOUTRXM);
        if (ret == 1 && pdo_entry != 0) {
            uint16_t obj_index = (pdo_entry >> 16) & 0xFFFF;
            uint8_t  sub_index = (pdo_entry >> 8) & 0xFF;
            uint8_t  length = pdo_entry & 0xFF;
            INFO_PRINT("\n  0x%04X:00 = 0x%08X -> 对象0x%04X:%02X, 长度%d位", 
                   idx, pdo_entry, obj_index, sub_index, length);
        } else if (pdo_entry == 0) {
            break;
        }
    }
    INFO_PRINT("\n=========================================\n");
}

// -------------------------- 读取从站同步管理器配置 --------------------------
void read_slave_sm_config(int slave_idx) {
    uint32_t sm_config;
    int ret;
    
    INFO_PRINT("\n===== 从站 %d 同步管理器配置 =====", slave_idx);
    for (int sm = 0; sm < 4; sm++) {
        ret = ecx_SDOread(&ecx_context, slave_idx, 0x1C00 + sm, 0x00, FALSE, sizeof(sm_config), &sm_config, EC_TIMEOUTRXM);
        if (ret == 1) {
            uint16_t start_addr = (sm_config >> 16) & 0xFFFF;
            uint16_t length = (sm_config >> 8) & 0xFF;
            uint8_t  type = sm_config & 0xFF;
            const char* type_str;
            
            switch(type) {
                case 0: type_str = "未使用"; break;
                case 1: type_str = "MBoxOut"; break;
                case 2: type_str = "MBoxIn"; break;
                case 3: type_str = "TxPDO"; break;
                case 4: type_str = "RxPDO"; break;
                default: type_str = "未知";
            }
            
            INFO_PRINT("\n  SM%d (0x%04X:00): 0x%08X -> 起始地址=0x%04X, 长度=%d字节, 类型=%s(%d)",
                   sm, 0x1C00 + sm, sm_config, start_addr, length, type_str, type);
        }
    }
    INFO_PRINT("\n=========================================\n");
}

// -------------------------- 读取所有从站配置 --------------------------
void read_slave_configurations() {
    INFO_PRINT("\n===================== 读取从站配置信息 =====================\n");
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        INFO_PRINT("\n----- 从站 %d 配置信息 -----", i);
        read_slave_pdo_mappings(i);
        read_slave_sm_config(i);
    }
    INFO_PRINT("============================================================\n");
}

// -------------------------- 新增：清除从站AL错误 --------------------------
int clear_slave_al_error(int slave_idx) {
    uint16_t clear_al = 0x0000;
    int ret = ecx_SDOwrite(&ecx_context, slave_idx, AL_STATUS_CODE, 0x00, FALSE,
                          sizeof(clear_al), &clear_al, EC_TIMEOUTRXM);
    return ret;
}

// -------------------------- ECAT RawData打印 --------------------------
void print_ecat_rawdata_pdo(int global_cycle) {
    INFO_PRINT("\n===================== ECAT PDO RawData（周期: %d） =====================\n", global_cycle);
    for (int motor_idx = 0; motor_idx < motor_count; motor_idx++) {
        if (!validate_motor_data_access(motor_idx)) {
            INFO_PRINT("Motor %d: 配置无效，跳过打印\n", motor_idx);
            continue;
        }
        motor_config_t *motor = &motor_configs[motor_idx];
        outputs_t *out_ptr = (outputs_t *)motor->outputs;
        inputs_t *in_ptr = (inputs_t *)motor->inputs;
        
        INFO_PRINT("Motor %d (Slave %d) | TPDO（输出）: ", motor_idx, motor->slave_index);
        for (int i = 0; i < motor->Obytes; i++) {
            INFO_PRINT("%02X ", motor->outputs[i]);
        }
        INFO_PRINT("| 控制字=0x%04X | 模式=0x%02X | 目标位置=%d | 触发功能=0x%04X | 目标速度=%d\n",
               out_ptr->controlword,
               out_ptr->mode_op,
               out_ptr->target_pos,
               out_ptr->touch_probe_func,
               out_ptr->target_vel);
        
        INFO_PRINT("Motor %d (Slave %d) | RPDO（输入）: ", motor_idx, motor->slave_index);
        for (int i = 0; i < motor->Ibytes; i++) {
            INFO_PRINT("%02X ", motor->inputs[i]);
        }
        INFO_PRINT("| 错误码=0x%04X | 状态字=0x%04X | 当前模式=0x%02X | 实际位置=%d | 实际速度=%d | 触发状态=0x%04X\n",
               in_ptr->error_code,
               in_ptr->statusword,
               in_ptr->mode_op_dis,
               in_ptr->actual_pos,
               in_ptr->actual_vel,
               in_ptr->touch_probe_status);
    }
    INFO_PRINT("=======================================================================\n");
}

// -------------------------- CSP控制主逻辑 --------------------------
void csp_control_main(char *ifname) {
    int i, chk;

    INFO_PRINT("\nStep 1: 初始化EtherCAT主站（网卡: %s）...\n", ifname);
    if (!ecx_init(&ecx_context, ifname)) {
        INFO_PRINT("ERROR: 主站初始化失败！请用root权限运行\n");
        goto cleanup;
    }

    INFO_PRINT("Step 2: 扫描并配置从站...\n");
    int slave_count = ecx_config_init(&ecx_context);
    if (slave_count <= 0) {
        INFO_PRINT("ERROR: 总线上未发现从站！\n");
        goto cleanup;
    }
    INFO_PRINT("INFO: 发现 %d 个EtherCAT从站\n", slave_count);

    // 新增：配置从站PDO映射
    INFO_PRINT("Step 2.5: 配置从站PDO映射...\n");
    for (i = 1; i <= slave_count; i++) {
        configure_slave_pdo_mapping(i);
    }

    read_slave_configurations();

    INFO_PRINT("Step 3: 配置PDO映射...\n");
    ecx_context.manualstatechange = 1;
    ecx_context.slavelist[0].state = EC_STATE_PRE_OP;
    ecx_writestate(&ecx_context, 0);
    ecx_statecheck(&ecx_context, 0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE * 4);
    if (ecx_context.slavelist[0].state != EC_STATE_PRE_OP) {
        INFO_PRINT("ERROR: 从站无法进入PRE_OP状态，PDO映射失败！\n");
        goto cleanup;
    }
    if (ecx_config_map_group(&ecx_context, IOmap, 0) <= 0) {
        INFO_PRINT("ERROR: PDO映射失败！\n");
        goto cleanup;
    }
    INFO_PRINT("INFO: PDO映射成功 - 输出: %d字节 | 输入: %d字节\n",
           ecx_context.slavelist[0].Obytes, ecx_context.slavelist[0].Ibytes);
    detailed_pdo_analysis();
    print_pdo_sizes();

    INFO_PRINT("Step 4: 配置DC同步...\n");
    ecx_configdc(&ecx_context);
    ecx_dcsync0(&ecx_context, 1, 1, EC_CYCLETIME, 0);
    INFO_PRINT("INFO: DC同步激活成功，周期: %d us\n", EC_CYCLETIME_US);

    INFO_PRINT("Step 5: 初始化电机配置...\n");
    init_motor_configs();
    if (motor_count == 0) {
        INFO_PRINT("ERROR: 未配置任何电机！\n");
        goto cleanup;
    }

    INFO_PRINT("Step 5.5: 验证电机初始状态...\n");
    for (int i = 0; i < motor_count; i++) {
        motor_config_t *motor = &motor_configs[i];
        inputs_t *in_ptr = (inputs_t *)motor->inputs;
        
        if (in_ptr) {
            INFO_PRINT("  Motor %d 初始状态: 状态字=0x%04X 模式=0x%02X 位置=%d 错误码=0x%04X\n", 
                   i, in_ptr->statusword, in_ptr->mode_op_dis, in_ptr->actual_pos, in_ptr->error_code);
        } else {
            INFO_PRINT("  Motor %d 初始状态: PDO指针无效\n", i);
        }
    }

    INFO_PRINT("Step 6: 切换从站到SAFE_OP状态...\n");
    ecx_context.slavelist[0].state = EC_STATE_SAFE_OP;
    ecx_writestate(&ecx_context, 0);
    ecx_statecheck(&ecx_context, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
    if (ecx_context.slavelist[0].state != EC_STATE_SAFE_OP) {
        INFO_PRINT("ERROR: 从站无法进入SAFE_OP状态！\n");
        for (i = 1; i <= slave_count; i++) {
            if (ecx_context.slavelist[i].state != EC_STATE_SAFE_OP) {
                INFO_PRINT("  Slave %d: 状态=0x%02X | AL错误码=0x%04X\n",
                       i, ecx_context.slavelist[i].state, ecx_context.slavelist[i].ALstatuscode);
            }
        }
        goto cleanup;
    }
    INFO_PRINT("INFO: 所有从站进入SAFE_OP状态！\n");

    INFO_PRINT("Step 7: 切换从站到OPERATIONAL状态...\n");
    expectedWKC = (ecx_context.grouplist[0].outputsWKC * 2) + ecx_context.grouplist[0].inputsWKC;
    INFO_PRINT("INFO: 预期工作计数器（WKC）: %d\n", expectedWKC);
    ecx_context.slavelist[0].state = EC_STATE_OPERATIONAL;
    ecx_writestate(&ecx_context, 0);
    chk = 200;
    do {
        ecx_send_processdata(&ecx_context);
        wkc = ecx_receive_processdata(&ecx_context, EC_TIMEOUTRET);
        ecx_statecheck(&ecx_context, 0, EC_STATE_OPERATIONAL, EC_CYCLETIME_US);
    } while (chk-- && (ecx_context.slavelist[0].state != EC_STATE_OPERATIONAL) && !exit_flag);
    if (ecx_context.slavelist[0].state != EC_STATE_OPERATIONAL) {
        INFO_PRINT("WARNING: 从站未进入OP状态，仍尝试发送CSP指令...\n");
    } else {
        INFO_PRINT("INFO: 所有从站进入OPERATIONAL状态，开始发送CSP指令！\n");
    }
    inOP = TRUE;
    for (i = 0; i < motor_count; i++) {
        motor_configs[i].is_operational = TRUE;
    }

    INFO_PRINT("\n===================== 开始CSP指令发送（Ctrl+C退出） =====================\n");
    int global_cycle = 0;
    while (!exit_flag) {
        global_cycle++;

        for (i = 0; i < motor_count; i++) {
            control_single_motor(i);
        }

        ecx_send_processdata(&ecx_context);
        wkc = ecx_receive_processdata(&ecx_context, EC_TIMEOUTRET);

        if (global_cycle % 10 == 0) {
            INFO_PRINT("全局周期: %4d | WKC: %d | ", global_cycle, wkc);
            for (i = 0; i < motor_count; i++) {
                print_motor_status(i);
            }
            INFO_PRINT("\r");
            fflush(stdout);
            needlf = TRUE;
        }

        if (global_cycle % 100 == 0) {
            print_ecat_rawdata_pdo(global_cycle);
        }

        osal_usleep(EC_CYCLETIME_US);
    }

cleanup:
    INFO_PRINT("\nStep 8: 退出清理，切换从站到INIT状态...\n");
    inOP = FALSE;
    if (ecx_context.slavelist != NULL) {
        ecx_context.slavelist[0].state = EC_STATE_INIT;
        ecx_writestate(&ecx_context, 0);
        ecx_statecheck(&ecx_context, 0, EC_STATE_INIT, EC_TIMEOUTSTATE * 2);
        ecx_close(&ecx_context);
    }
}

// -------------------------- 辅助函数 --------------------------
void print_usage() {
    INFO_PRINT("用法: ./multi_motor_csp <以太网适配器名称>\n");
    INFO_PRINT("示例: ./multi_motor_csp enp2s0\n");
    INFO_PRINT("注意: 必须用root权限运行（访问网卡需要）\n");
}

void signal_handler(int sig) {
    if (sig == SIGINT) {
        INFO_PRINT("\n\nINFO: 收到Ctrl+C，准备退出...\n");
        exit_flag = TRUE;
    }
}